/*
This script is designed to upload static thrust data 
to the RCbenchmark database. The script performs 
throttle steps as shown below:

First, the script does a quick pre-qualification ramp
up to specified throttle limit, to automatically 
determine the min/max throttle. The min throttle is
where the motor starts to spin while the max throttle
is where the maximum rpm was reached. This helps avoid
duplicate data points at the same RPM.

Then, the script does steps and stops at each step
to allow the system to stabilize.

The '.' represents a sample is recorded. A sample is
first recorded at the beginning, after taring the load
cells and the ESC initialization.
 
 ^ Motor      _throttleLimit
 | input              /|__.     _throttle of
 |                   /     \__.     max RPM
 |                  /          \__.      
 |                 /               \__.
 |                / _motor starts      \__.
 |  _escStart __./                              
 |_______________________________________> time
 
At the start of each step there is a time to 
stabilize the rotation speed, after which a data
sample is taken. 

This test can take a while to run, therefore there
is an option to allow the motor to cool down 
between each step. There is a setting to also perform
the cooldown only at the highest throttle steps.

*/

////////// Script parameters ///////// */

var escStart = @@{
  "type": "integer",
  "min": 700,
  "max": 1600,
  "name": "ESC init value (µs)",
  "default": 1000
}##; // ESC idle value
var escInitTime = @@{
  "type": "float",
  "min": 1,
  "max": 150,
  "name": "ESC init time (s)",
  "hint": "How long to keep the ESC init value before starting the test. Usually 4 seconds is sufficient, but a longer time may be useful to delay the start of a test.",
  "default": 4
}##; // Hold time at ESC idle
var throttleLimit = @@{
  "type": "integer",
  "min": 700,
  "max": 2300,
  "name": "Maximum throttle value (µs)",
  "hint": "Set lower if the components cannot handle full power without damage or without hitting cutoff limits.",
  "default": 2000
}##; // Max. input value

// step parameters
var params = {
    steps_qty: @@{
  "type": "integer",
  "min": 2,
  "max": 100,
  "name": "Number of steps",
  "hint": "More steps will yield better resolution data, but not always possible if testing on a fast-discharging battery. Minimum is 2, representing one step at the max RPM and another at the motor start value.",
  "default": 20
}##, // Number of steps
    settlingTime_s: @@{
  "type": "float",
  "min": 1,
  "max": 20,
  "name": "Settling time (s)",
  "hint": "Ensure the settling time is sufficiently long to let the sensors stabilize after a change of throttle.",
  "default": 4
}##, // Settling time before measurement
    cooldownTime_s: @@{
  "type": "float",
  "min": 0,
  "max": 1200,
  "name": "Cooling time each step (s)",
  "hint": "Use this setting if the components under test cannot withstand a continuous throttle without overheating.",
  "default": 0
}##, // If the motor needs to cool down between steps
    cooldownThrottle_us: @@{
  "type": "integer",
  "min": 700,
  "max": 1600,
  "name": "Cooling throttle (µs)",
  "hint": "A slowly spining motor may actually cool down faster than a stopped motor due to air convection around the hot motor coils.",
  "default": 1000
}##, // Cool down faster when slowly spinning
    cooldownMinThrottle: @@{
  "type": "integer",
  "min": 700,
  "max": 2300,
  "name": "Cooling min. throttle",
  "hint": "Testing time can increase dramatically if cooling even at low throttles. This setting will activate cooling only at high throttle steps.",
  "default": 1600
}##, // Cool down faster when slowly spinning
    max_slew_rate_us_per_s: @@{
  "type": "integer",
  "min": 0,
  "max": 10000,
  "name": "Max. slew rate (µs/s)",
  "hint": "This sets how fast the throttle signal is allowed to change. It helps avoid high torque when transitioning between two steps. Also helps prevent damage to the power supply by limiting braking speed. Set to zero to disable.",
  "default": 100
}## // Limits torque from throttle changes, but adds extra test time
};

var save_CSV_file = @@{
  "type": "boolean",
  "name": "Save data to CSV file?",
  "hint": "For your own records, you may optionally save the data to a CSV file, in addition to uploading to the database.",
  "default": false
}##; // Save data to CSV file
   
var samplesAvg = @@{
  "type": "integer",
  "min": 10,
  "max": 5000,
  "name": "Samples to average",
  "hint": "Minimum is 10, but it can be set higher for higher quality results, especially if there are a lot of vibrations.",
  "default": 100
}##;     // Number of samples to average

//////// Beginning of the script /////////

if(rcb.getBoardVersion() === undefined){
  rcb.console.error("Error: please connect a supported tool before running this script.");
}

var output = "esc";
if(rcb.getBoardVersion() === "Series 1780"){
  output = "escA";
}

if(escStart >= throttleLimit || params.cooldownThrottle_us >= throttleLimit){
  rcb.console.error("Error: invalid throttle parameters, ensure throttleLimit is the maximum.");
}

var firstRowAdded = false;
var rampingDown = false;
var rampThrottle = escStart;
var minThrottle;
var maxThrottle;
var maxRPM;

//Starting a new CSV log file
if(save_CSV_file){
  rcb.files.newLogFile({prefix: @@@FILENAME###});
}

//Tare the load cells and the current sensor
function tare(callback){
  rcb.sensors.tareLoadCells(function(){
    //taring current has an effect only in the Series 1780
    rcb.sensors.tareCurrent(callback);
  });
}

initESC(function(){
  tare(function(){
    takeSample(previewTest);
  });
});

//Arms the ESC
function initESC(callback){
    //ESC initialization
    rcb.console.print("Initializing ESC...");
    rcb.database.log("ESC init value (µs): " + escStart);
    rcb.database.log("Throttle max limit (µs): " + throttleLimit);
    rcb.output.set(output, escStart);
    rcb.wait(callback, escInitTime);
}

//Calculates the rpm value depending on if using the optical or electrical probe
function getRPM(result){
  var rpm = result.motorOpticalSpeed.workingValue;
  if(result.motorElectricalSpeed){
    rpm = math.max(result.motorElectricalSpeed.workingValue, rpm);
  }
  return rpm;
}

//Starts the pre-scan ramp up, to determine the min and max throttle
function previewTest(){
  var slewRate = params.max_slew_rate_us_per_s;
  var lastTime = window.performance.now();
  var rampDone = false;
  rcb.console.setVerbose(false);
  
  // Add a delay at the top of the ramp to ensure the maximum throttle is sampled
  function finishRamp(){
    rcb.wait(function(){
      rampDone = true;
    }, params.settlingTime_s);
  }
  
  function readSensor(){
    rcb.sensors.read(readDone, 1);
  }

  function readDone(result){
    var currTime = window.performance.now();
    var rampTime;
    // find out at which throttle the motor starts to spin
    if(!minThrottle){
      if(getRPM(result)>0){
        minThrottle = rampThrottle;
        rcb.database.log("Motor starts at " + minThrottle + " (automatically detected by the script)");
        minThrottle += 10; // helps avoid motor stuttering at start.
        statusText = rcb.console.print("Ramping up to find throttle of max RPM...");
        throttleText = rcb.console.print("");
        var rampRate = 100; // the minimum rate at which we want to increase the throttle for this code to work well
        if(slewRate > 0){
          rampRate = math.min(slewRate, rampRate); // respect the user's own slew rate limit
        }
        rampTime = (throttleLimit - minThrottle) / rampRate;
        if(minThrottle >= throttleLimit-10){
          rcb.console.error("Error: the throttle limit is too close to the motor starting value");
        }
        rcb.output.ramp(output, minThrottle, throttleLimit, rampTime, finishRamp);
        readSensor();
      }else{
        rampThrottle += 1;
        if(rampThrottle >= throttleLimit){
          rcb.console.error("No motor rotation detected. Check your setup and ensure the RPM sensor is working.");
        }
        rcb.output.set(output, rampThrottle);
        rcb.console.overwrite(rampThrottle, throttleText);
        rcb.wait(readSensor, 0.02);
      }
    }else{
      // As long as RPM is increasing, we save the new maxThrottle value
      var throttle = result[output].workingValue;
      rcb.console.overwrite(throttle, throttleText);
      if(!rampDone && (!maxRPM || getRPM(result) > maxRPM + 2)){ //some tolerance to RPM noise
        maxThrottle = throttle;
        maxRPM = getRPM(result);
      }
      if(rampDone){
        rcb.database.log("RPM maxed at " + maxThrottle + " at " + maxRPM + " RPM (autodetect)");
        rcb.console.print("Sampling at max throttle...");
        throttleText = rcb.console.print("");
        rcb.output.set(output, maxThrottle);
        rcb.console.setVerbose(true);
        steps();
      }else{
        readSensor();
      }
    }
  }
  var statusText = rcb.console.print("Ramping up to find motor start value...");
  var throttleText = rcb.console.print("");
  readSensor();
}

// Records a sample to database
function takeSample(callback){
    rcb.sensors.read(function (result){
        var rpm = getRPM(result);
        if(result.thrust.workingValue < -0.05){
          rcb.console.error("At this time we only support positive thrust values. Please reverse the thrust sign from the utilities tab.");
        }
        if(result.torque && result.torque.workingValue < -0.01){
          rcb.console.error("At this time we only support positive torque values. Please reverse the torque sign from the utilities tab.");
        }
        if(result.current.workingValue < -0.1){
          rcb.console.error("At this time we only support positive current values.");
        }
        if(!firstRowAdded && rpm > 0){
          rcb.console.error("The first row of the data should be at zero rpm.");
        }
        // do not add more than one rpm=0 rows
        if(!firstRowAdded || rpm > 0){
          rcb.database.addData(result);
          firstRowAdded = true;
        } 
      
        if(save_CSV_file){
          rcb.files.newLogEntry(result, callback);
        }else{
          callback();
        }
    }, samplesAvg);
}

// Start the steps function, going downwards
function steps(){
    var minStep = (minThrottle + (maxThrottle - minThrottle) * 0.05); // At very low throttle, the signal-to-noise ratio is too high to be meaningful
    rcb.output.steps2(output, maxThrottle, minStep, stepFct, endFct, params);
}

// The following function will be executed at each step.
function stepFct(nextStepFct){
    takeSample(nextStepFct);  
}

// Called at the end of the steps
function endFct(){
    // stop the motor
    rcb.output.set(output,escStart);
    rcb.wait(function(){
      rcb.database.submit(rcb.endScript);
    }, 1);
}
